#!/usr/bin/env python
# coding=utf-8

import rospy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
import cv2, cv_bridge
import numpy as np
import matplotlib.pyplot as plt
import glob
import logging  
import math
from get_line import process_image
from img_remote import img_pub

class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        # 订阅usb摄像头
        # 订阅深度相机
        self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
        # self.image_sub = rospy.Subscriber("/camera/depth/image_raw", Image,self.image_callback)
        self.image_pub = rospy.Publisher("/camera/rgb/line_detect", Image, queue_size=1)
        self.twist = Twist()
        # 获取ROS的root logger，这会自动继承ROS的日志级别和格式化设置  
        self.logger = logging.getLogger('/camera/rgb/line_detect')  
        # 设置日志级别，这里设置为INFO，这样INFO及以上级别的日志都会被记录  
        self.logger.setLevel(logging.INFO)  
        #图片远程传输到上位机
        self.img_pub = img_pub("192.168.0.102",5555)
    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        #image = cv2.resize(image, (640,480), interpolation=cv2.INTER_AREA)#提高帧率
        #rospy.loginfo("1234555!")
        out_image = process_image(image)
        self.img_pub.send_img(out_image)
        # cv2.imshow("out",out_image)
        # cv2.waitKey(1)
        # out_image = image
        #out_image = self.bridge.cv2_to_imgmsg(out_image,'bgr8')
        #out_image.header.stamp = rospy.Time.now()
        #self.image_pub.publish(out_image)
        #rospy.sleep(1) 


rospy.init_node("opencv")
follower = Follower()
rospy.spin()
